#pragma once

#include <mutex>
#include <zcm/zcm-cpp.hpp>
#include "Robot.hpp"
#include "ZCM_Data.hpp"
#include "config.hpp"
#include "state_estimator_zcmt.hpp"
#include "rc_command_zcmt.hpp"
#include "upper_joint_control_zcmt.hpp"
#include "upper_joint_state_zcmt.hpp"
#include "system_init_zcmt.hpp"
#include "chassis_control_zcmt.hpp"
#include "chassis_state_zcmt.hpp"
#include "waist_control_zcmt.hpp"
#include "waist_state_zcmt.hpp"
#include "gripper_control_zcmt.hpp"
#include "gripper_state_zcmt.hpp"
#include "head_control_zcmt.hpp"
#include "head_state_zcmt.hpp"
#include "arm_actions_zcmt.hpp"




class H1Robot : public RobotBase {
public:
    // 使用共享内存通信
    explicit H1Robot();
    
    // 使用UDP多播通信
    explicit H1Robot(const std::string& udp_url);
    
    // 获取当前通信方式
    bool isUsingSharedMemory() const { return use_shared_memory_; }


    // ===== 模式管理 =====
    // 连接机器人
    // 返回值：true - 成功，false - 失败
    bool robot_connect();
    // 切换控制模式
    // 返回值：true - 成功，false - 失败
    bool switchControlMode(MotorControlMode new_mode);
    
    // 初始化/反初始化，返回是否成功，失败报错，阻塞到收到状态为止，阻塞期间不能进行其他操作
    //初始化和反初始化互斥
    // 初始化机器人，运动接口
    // 返回值：true - 成功，false - 失败
    bool robot_init() ;
    // 反初始化机器人，运动接口
    // 返回值：true - 成功，false - 失败
    bool robot_deinit() ;
  
    // ===== LOW-LEVEL 接口 =====


    // 直接控制电机
    //参数说明：motor_id - 电机索引,0-22
    //          control - 电机控制参数,如下
                    
    // typedef struct{
    //     uint16_t Control_Mode;   //位置，力矩模式
    //     float KP;
    //     float KD;
    //     float Position;
    //     float Speed;
    //     float Torque;
    // } __attribute__((packed)) Motor_Control;

    bool setMotorControl_low(EtherCAT_Motor_Index motor_id, const Motor_Control& control);

    // 设置底盘
    // 参数说明：chassis_id - 底盘电机索引, 左轮:0, 右轮:1
    //          control - 电机控制参数,仅支持控制速度
    bool setChassis_low(int chassis_id, const Motor_Control& control);
    // 设置腰部
    // 参数说明：waist_id - 腰部电机索引, 升降2，俯仰:3, 旋转:4
    //          control - 电机控制参数，升降仅支持控制位置，俯仰和旋转支持控制速度，位置和力矩
    bool setWaist_low(int waist_id, const Motor_Control& control);
    // 设置头部
    // 参数说明：head_id - 头部电机索引, 旋转:5, 俯仰:6
    //          control - 电机控制参数，支持控制位置、速度和力矩
    bool setHead_low(int head_id, const Motor_Control& control);
    // 设置手臂
    // 参数说明：arm_id 关节索引, 7-13 - 左臂 肩部->手腕，15-21 右臂 肩部->手腕
    //          control - 电机控制参数，支持控制位置、速度和力矩
    bool setArm_low(int arm_id, const Motor_Control& control);
    // 设置夹爪
    // 夹爪实际为手部的最后一个电机
    // 参数说明：gripper_id - 夹爪电机索引,14 - 左手，22 - 右手
    //          control - 电机控制参数，支持控制位置、速度和力矩
    bool setGripper_low(int gripper_id, const Motor_Control& control);



    // ===== HIGH-LEVEL 接口 =====


    // // 模拟手柄控制
    // bool setJoystick_high(const xbox_map_zcmt& control);//todo bottom_line需要添加订阅遥控器状态
    

                       
    // 设置底盘线速度和角速度
    //          ChassisMotion - 运动模式, 停止:0, 前进:1, 后退:2, 左转:3, 右转:4
    //          control - 底盘控制参数，线速度:control.speed[0],角速度:control.speed[1]
    // 返回值：true - 成功，false - 失败
    bool setChassis_high(ChassisMotion chassis_id, const Motor_Control& control);

    // 设置腰部姿态
    bool setWaist_high(int waist_id, const Motor_Control& control);

    // 设置头部姿态,实际与低层控制相同
    bool setHead_high(int head_id, const Motor_Control& control);


    // 设置手臂末端姿态
    bool setArm_high(ArmAction arm, arm_actions_zcmt& action);

    // 设置夹爪末端姿态，实际与低层控制相同
    bool setGripper_high(int gripper_id, const Motor_Control& control);

    // ===== 状态监控 =====
    
    // 获取机器人基本信息
    // 返回值：true - 成功，false - 失败
    //可以在机器人未初始化时调用，以及用于判断SDK是否连接上了机器人
    bool getRobotInfo(robot_info_zcmt& state);

    // 获取电机状态
    // 参数说明：motor_id - 电机索引, state - 电机状态结构
    bool getMotorState(int motor_id, Motor_Information& state);
    // 获取底盘状态
    bool getChassisState(int chassis_id, Motor_Information& state);
    // 获取腰部状态
    bool getWaistState(int waist_id, Motor_Information& state);
    // 获取头部状态
    bool getHeadState(int head_id, Motor_Information& state);
    // Motor_Information
    bool getArmState(int joint_id, Motor_Information& state);
    // 获取夹爪状态
    bool getGripperState(int gripper_id, Motor_Information& state);

    bool getJoystickState(xbox_map_zcmt& state);

    bool getIMU_State(state_estimator_zcmt& state);


    bool isRobotConnected(); // 检查机器人是否连接成功

    // 获取当前模式
    MotorControlMode getCurrentMode();
    // 获取当前初始化状态
    // 返回值：-1 - 未初始化，0 - 初始化中，1 - 已初始化, 2 - 反初始化中，3 - 已反初始化
    InitState getInitState();

    
    
    // // 获取错误码    
    // ErrorCode getLastError() const override;

    // 错误处理
    void handleError(ErrorCode code, const std::string& message);

    // ===== 传感器数据 =====
private:
    // ZCM 回调

    // void handleStateUpdate(const zcm::ReceiveBuffer* rbuf, 
    //                       const std::string& channel, 
    //                       const h1_state_t* msg);
    // void handlePinocchioResponse(const zcm::ReceiveBuffer* rbuf,
    //                             const std::string& channel,
    //                             const h1_joint_command_t* msg);
    
    // 内部状态

    // ZCM数据处理类实例
    ZCM_Data zcm_data_;


    bool is_robot_connected = false; // 是否连接到机器人

    // 通信方式：true=共享内存，false=UDP多播
    bool use_shared_memory_;

    //直接用ZCM_Data::system_init_zcmt::InitState代替
    // //0表示未初始化，1表示初始化中，2表示已初始化,3表示反初始化中，4表示已反初始化
    // InitState init_state=InitState::Uninit;

    MotorControlMode current_mode_{UNINITIALIZED};
    ErrorCode last_error_{NO_ERROR};
    std::string last_error_message_;
    std::vector<int>error_count;
    // 互斥锁保护共享数据   
     mutable std::mutex state_mutex_; 
};